%% 清空变量
clear;
clc
%% 定义变量
axisStart = [0 0 0];
axisLWH = [1000 1000 1000];

%定义障碍物
cubeInfo.exist = 0;
cylinderInfo.exist = 0;
sphereInfo.exist = 0;

pathPoint = [0 0 0;
             1000 1000 1000];

cubeInfo = createCubeObject(cubeInfo);%创建长方体信息
cylinderInfo=createCylinderObject(cylinderInfo);%创建圆柱体信息
sphereInfo=createSphereObject(sphereInfo);%创建球体信息

%% 画图
figure(1)
colorMatCube=[1 0 0];
colorMatCylinder=[0 1 0];
colorMatSphere=[0 0 1];
pellucidity=0.6; %透明度
hold on;
scatter3(pathPoint(1,1),pathPoint(1,2),pathPoint(1,3),'MarkerEdgeColor','k','MarkerFaceColor',[0 .75 .75]);
scatter3(pathPoint(end,1),pathPoint(end,2),pathPoint(end,3),'MarkerEdgeColor','k','MarkerFaceColor','b');
%画长方体
drawCubeObject(cubeInfo,pellucidity,colorMatCube);
%画圆柱体
drawCylinderObject(cylinderInfo,pellucidity,colorMatCylinder);
%画球体
drawSphereObject(sphereInfo,colorMatSphere,pellucidity);

text(pathPoint(1,1),pathPoint(1,2),pathPoint(1,3),'起点');
text(pathPoint(end,1),pathPoint(end,2),pathPoint(end,3),'终点');
view(3)
grid on;
axis equal;
axis([0 1000 0 1000 0 1000])
xlabel('x')
ylabel('y')
zlabel('z')


%% 寻找路径
totalPath=[];
for k1=1:size(pathPoint,1)-1
    startPoint=pathPoint(k1,:);
    goalPoint=pathPoint(k1+1,:);
    Path=RRT(startPoint,axisStart,axisLWH,goalPoint,cubeInfo,cylinderInfo,sphereInfo);

    if ~isempty(Path)
        for k2=1:size(Path,1)-1
            line([Path(k2,1) Path(k2+1),1],[Path(k2,2) Path(k2+1,2)],[Path(k2,3) Path(k2+1,3)],'LineWidth',1,'Color','red')
        end
        totalPath=[totalPath;Path];
    end

end

